////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNMath/inc/WNExtended.h"
#include "WNMath/inc/WNBasic.h"
#include "WNMath/inc/WNPoint2.h"
#include "WNMath/inc/WNPoint3.h"
#include "WNMath/inc/WNVector2.h"
#include "WNMath/inc/WNVector3.h"
#include "WNMath/inc/WNQuaternion.h"
#include "WNMath/inc/WNPlane.h"
#include "WNMath/inc/WNRay2.h"
#include "WNMath/inc/WNRay3.h"
#include "WNMath/inc/WNLine2.h"
#include "WNMath/inc/WNLine3.h"
#include "WNMath/inc/WNSegment2.h"
#include "WNMath/inc/WNSegment3.h"

namespace WNMath {
    WN_FLOAT32 WNDot(const WNVector2& _vector1, const WNVector2& _vector2) {
        const WNVector2 vec1 = _vector1;
        const WNVector2 vec2 = _vector2;

        return((vec1.x * vec2.x) + (vec1.y * vec2.y));
    }

    WN_FLOAT32 WNDot(const WNVector3& _vector1, const WNVector3& _vector2) {
        const WNVector3 vec1 = _vector1;
        const WNVector3 vec2 = _vector2;

        return((vec1.x * vec2.x) + (vec1.y * vec2.y) + (vec1.z * vec2.z));
    }

    WN_FLOAT32 WNDot(const WNQuaternion& _quat1, const WNQuaternion& _quat2) {
        const WNQuaternion quat1 = _quat1;
        const WNQuaternion quat2 = _quat2;

        return((quat1.x * quat2.x) + (quat1.y * quat2.y) +
               (quat1.z * quat2.z) + (quat1.w * quat2.w));
    }

    WN_FLOAT32 WNCCW(const WNVector2& _vector1, const WNVector2& _vector2) {
        const WNVector2 vec1 = _vector1;
        const WNVector2 vec2 = _vector2;

        return((vec1.x * vec2.x) + (vec1.y * vec2.y));
    }

    WNVector3 WNCross(const WNVector3& _vector1, const WNVector3& _vector2) {
        const WNVector3 vec1 = _vector1;
        const WNVector3 vec2 = _vector2;

        return(WNVector3(vec1.y * vec2.z - vec1.z * vec2.y,
                         vec1.z * vec2.x - vec1.x * vec2.z,
                         vec1.x * vec2.y - vec1.y * vec2.x));
    }

    WNVector2 WNLerp(const WNVector2& _start, const WNVector2& _end, WN_FLOAT32 _amount) {
        const WNVector2 vec1 = _start;
        const WNVector2 vec2 = _end;

        if (_amount <= 0.0f) {
            return(vec1);
        } else if (_amount >= 1.0f) {
            return(vec2);
        }
        
        return(vec1 + (_amount * (vec2 - vec1)));
    }

    WNVector3 WNLerp(const WNVector3& _start, const WNVector3& _end, WN_FLOAT32 _amount) {
        const WNVector3 vec1 = _start;
        const WNVector3 vec2 = _end;

        if (_amount <= 0.0f) {
            return(vec1);
        } else if (_amount >= 1.0f) {
            return(vec2);
        }

        return(vec1 + (_amount * (vec2 - vec1)));
    }

    WNQuaternion WNLerp(const WNQuaternion& _quatStart, const WNQuaternion& _quatEnd, WN_FLOAT32 _amount) {
        const WNQuaternion quat1 = _quatStart;
        const WNQuaternion quat2 = _quatEnd;

        if (_amount <= 0.0f) {
            return(quat1);
        } else if (_amount >= 1.0f) {
            return(quat2);
        }

        return(quat1 + (_amount * (quat2 - quat1)));
    }

    WNVector2 WNSLerp(const WNVector2& _start, const WNVector2& _end, WN_FLOAT32 _amount) {
        const WNVector2 vec1 = _start;
        const WNVector2 vec2 = _end;

        if (_amount <= 0.0f) {
            return(vec1);
        } else if (_amount >= 1.0f) {
            return(vec2);
        }

        const WN_FLOAT32 cosOmega = WNDot(vec1, vec2);

        WN_FLOAT32 scale1;
        WN_FLOAT32 scale2;

        if ((1.0f - cosOmega) > 1e-6) {
            const WN_FLOAT32 omega = WNACos(cosOmega);
            const WN_FLOAT32 sinOmega = WNSin(omega);

            scale1 = WNSin((1.0f - _amount) * omega) / sinOmega;
            scale2 = WNSin(_amount * omega) / sinOmega;
        } else {
            scale1 = 1.0f - _amount;
            scale2 = _amount;
        }

        return((vec1 * scale1) + (vec2 * scale2));
    }

    WNVector3 WNSLerp(const WNVector3& _start, const WNVector3& _end, WN_FLOAT32 _amount) {
        const WNVector3 vec1 = _start;
        const WNVector3 vec2 = _end;

        if (_amount <= 0.0f) {
            return(vec1);
        } else if (_amount >= 1.0f) {
            return(vec2);
        }

        const WN_FLOAT32 cosOmega = WNDot(vec1, vec2);

        WN_FLOAT32 scale1;
        WN_FLOAT32 scale2;

        if ((1.0f - cosOmega) > 1e-6) {
            const WN_FLOAT32 omega = WNACos(cosOmega);
            const WN_FLOAT32 sinOmega = WNSin(omega);

            scale1 = WNSin((1.0f - _amount) * omega) / sinOmega;
            scale2 = WNSin(_amount * omega) / sinOmega;
        } else {
            scale1 = 1.0f - _amount;
            scale2 = _amount;
        }

        return((vec1 * scale1) + (vec2 * scale2));
    }

    WNQuaternion WNSLerp(const WNQuaternion& _quatStart, const WNQuaternion& _quatEnd, WN_FLOAT32 _amount) {
        const WNQuaternion quat1 = _quatStart;
        const WNQuaternion quat2 = _quatEnd;

        if (_amount <= 0.0f) {
            return(quat1);
        } else if (_amount >= 1.0f) {
            return(quat2);
        }

        WN_FLOAT32 cosOmega = WNDot(quat1, quat2);

        WNQuaternion quatInverse = quat2;

        if (cosOmega < 0.0f) {
            quatInverse = -quatInverse;
            cosOmega = -cosOmega;
        }

        WN_FLOAT32 scale1;
        WN_FLOAT32 scale2;

        if ((1.0f - cosOmega) > 0.000001f) {
            const WN_FLOAT32 omega = WNACos(cosOmega);
            const WN_FLOAT32 sinOmega = 1.0f / WNSin(omega);

            scale1 = WNSin((1.0f - _amount) * omega) * sinOmega;
            scale2 = WNSin(_amount * omega) * sinOmega;
        } else {
            scale1 = 1.0f - _amount;
            scale2 = _amount;
        }

        return((scale1 * quat1) + (scale2 * quatInverse));
    }

    WN_FLOAT32 WNAngleBetween(const WNVector2& _vector1, const WNVector2& _vector2) {
        const WNVector2 vec1 = _vector1.GetNormalized();
        const WNVector2 vec2 = _vector2.GetNormalized();

        return(WNACos(WNDot(vec1, vec2)));
    }

    WN_FLOAT32 WNAngleBetween(const WNVector3& _vector1, const WNVector3& _vector2) {
        const WNVector3 vec1 = _vector1.GetNormalized();
        const WNVector3 vec2 = _vector2.GetNormalized();

        return(WNACos(WNDot(vec1, vec2)));
    }

    WN_FLOAT32 WNDistance(const WNPoint2& _point1, const WNPoint2& _point2) {
        const WNVector2 vec = _point1 - _point2;

        return(vec.Length());
    }

    WN_FLOAT32 WNDistance(const WNPoint3& _point1, const WNPoint3& _point2) {
        const WNVector3 vec = _point1 - _point2;

        return(vec.Length());
    }

    WN_FLOAT32 WNDistance(const WNPlane& _plane, const WNPoint3& _point) {
        return(WNDot(static_cast<WNVector3>(_point), _plane.Normal()) - _plane.d);
    }

    WN_FLOAT32 WNDistanceSquared(const WNPoint2& _point1, const WNPoint2& _point2) {
        const WNVector2 vec = _point1 - _point2;

        return(vec.LengthSquared());
    }

    WN_FLOAT32 WNDistanceSquared(const WNPoint3& _point1, const WNPoint3& _point2) {
        const WNVector3 vec = _point1 - _point2;

        return(vec.LengthSquared());
    }

    WNPoint2 WNMidPoint(const WNPoint2& _point1, const WNPoint2& _point2) {
        return(WNPoint2((_point1.x + _point2.x) * 0.5f,
                          (_point1.y + _point2.y) * 0.5f));
    }

    WNPoint3 WNMidPoint(const WNPoint3& _point1, const WNPoint3& _point2) {
        return(WNPoint3((_point1.x + _point2.x) * 0.5f,
                          (_point1.y + _point2.y) * 0.5f,
                          (_point1.z + _point2.z) * 0.5f));
    }

    WN_BOOL WNIntersect(const WNLine2& _line1, const WNLine2& _line2, WNPoint2& _intersection) {
        const WNVector2 l1d = _line1.Direction();
        const WNVector2 l2d = _line2.Direction();
        const WN_FLOAT32 dot = WNDot(l1d, l2d);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WNPoint2 l10 = _line1.mPoint1;
        const WNPoint2 l20 = _line1.mPoint1;
        const WN_FLOAT32 invd = 1.0f / (l1d.y * l2d.x - l1d.x * l1d.y);
        const WN_FLOAT32 l0diffy = l20.y - l10.y;
        const WN_FLOAT32 l0diffx = l10.x - l20.x;
        const WN_FLOAT32 t = (l2d.x * l0diffy + l2d.y * l0diffx) * invd;
        const WN_FLOAT32 s = (l1d.x * l0diffy + l1d.y * l0diffx) * invd;

        _intersection = l10 + (l1d * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNRay2& _ray1, const WNRay2& _ray2, WNPoint2& _intersection) {
        const WNVector2 r1d = _ray1.direction;
        const WNVector2 r2d = _ray2.direction;
        const WN_FLOAT32 dot = WNDot(r1d, r2d);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WNPoint2 r10 = _ray1.location;
        const WNPoint2 r20 = _ray2.location;
        const WN_FLOAT32 invd = 1.0f / (r1d.y * r2d.x - r1d.x * r1d.y);
        const WN_FLOAT32 r0diffy = r20.y - r10.y;
        const WN_FLOAT32 r0diffx = r10.x - r20.x;
        const WN_FLOAT32 t = (r2d.x * r0diffy + r2d.y * r0diffx) * invd;
        const WN_FLOAT32 s = (r1d.x * r0diffy + r1d.y * r0diffx) * invd;

        if (t < 0.0f && s < 0.0f) {
            return(WN_FALSE);
        }

        _intersection = r10 + (r1d * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNSegment2& _segment1, const WNSegment2& _segment2, WNPoint2& _intersection) {
        const WNVector2 s1d = _segment1.Direction();
        const WNVector2 s2d = _segment2.Direction();
        const WN_FLOAT32 dot = WNDot(s1d, s2d);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WNPoint2 s10 = _segment1.start;
        const WNPoint2 s20 = _segment2.start;
        const WN_FLOAT32 invd = 1.0f / (s1d.y * s2d.x - s1d.x * s1d.y);
        const WN_FLOAT32 s0diffy = s20.y - s10.y;
        const WN_FLOAT32 s0diffx = s10.x - s20.x;
        const WN_FLOAT32 t = (s2d.x * s0diffy + s2d.y * s0diffx) * invd;
        const WN_FLOAT32 s = (s1d.x * s0diffy + s1d.y * s0diffx) * invd;

        if ((t < 0.0f && s < 0.0f) ||
             (t > 1.0f && s > 1.0f)) {
            return(WN_FALSE);
        }

        _intersection = s10 + (s1d * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNLine3& _line, const WNPlane& _plane, WNPoint3& _intersection) {
        const WNPoint3 l0 = _line.mPoint1;
        const WNVector3 ld = _line.Direction();        
        const WNVector3 pn = _plane.Normal();
        const WN_FLOAT32 dot = WNDot(pn, ld);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WN_FLOAT32 v0 = -(WNDot(pn, *reinterpret_cast<const WNVector3*>(&l0)) + _plane.d);
        const WN_FLOAT32 t = v0 / dot;

        _intersection = l0 + (ld * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNRay3& _ray, const WNPlane& _plane, WNPoint3& _intersection) {
        const WNPoint3 r0 = _ray.location;
        const WNVector3 rd = _ray.direction;
        const WNVector3 pn = _plane.Normal();
        const WN_FLOAT32 dot = WNDot(pn, rd);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WN_FLOAT32 v0 = -(WNDot(pn, *reinterpret_cast<const WNVector3*>(&r0)) + _plane.d);
        const WN_FLOAT32 t = v0 / dot;

        if (t < 0.0f) {
            return(WN_FALSE);
        }

        _intersection = r0 + (rd * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNSegment3& _segment, const WNPlane& _plane, WNPoint3& _intersection) {
        const WNPoint3 r0 = _segment.start;
        const WNVector3 rd = _segment.Direction();
        const WNVector3 pn = _plane.Normal();
        const WN_FLOAT32 dot = WNDot(pn, rd);

        if (dot == 0.0f) {
            return(WN_FALSE);
        }

        const WN_FLOAT32 v0 = -(WNDot(pn, *reinterpret_cast<const WNVector3*>(&r0)) + _plane.d);
        const WN_FLOAT32 t = v0 / dot;

        if (t < 0.0f || t > 1.0f) {
            return(WN_FALSE);
        }

        _intersection = r0 + (rd * t);

        return(WN_TRUE);
    }

    WN_BOOL WNIntersect(const WNPlane& _plane1, const WNPlane& _plane2, WNLine3& _intersection) {
        const WN_FLOAT32 n1ls = _plane1.Normal().LengthSquared();
        const WN_FLOAT32 n2ls = _plane2.Normal().LengthSquared();
        const WN_FLOAT32 dotn1n2 = WNDot(_plane1.Normal(), _plane2.Normal());
        const WN_FLOAT32 det = n1ls * n2ls - dotn1n2 * dotn1n2;

        if (WNAbs(det) == 0.0f) {
            return(WN_FALSE);
        }

        const WN_FLOAT32 invDet = 1.0f / det;
        const WN_FLOAT32 f0 = (dotn1n2 * _plane2.d - n2ls * _plane1.d) * invDet;
        const WN_FLOAT32 f1 = (dotn1n2 * _plane1.d - n1ls * _plane2.d) * invDet;
        const WNVector3 dir = WNCross(_plane1.Normal(), _plane2.Normal());
        const WNPoint3 p = static_cast<WNPoint3>(f0 * _plane1.Normal() + f1 * _plane2.Normal());

        _intersection.mPoint1 = p;
        _intersection.mPoint2 = p + dir;

        return(WN_TRUE);
    }
}